/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a33 copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Quaternion.h"

namespace Robotic { namespace Math{
	Quaternion::Quaternion()
	{
		S  = 1.0;
		V  = ColumnVector(3);
		V.SetZero();
	}

	Quaternion::Quaternion(const double angle, const ColumnVector & axis)
	{
		if (axis.GetCount() != 3)
			throw(new std::exception("Argument axis's count must be 3."));

		// make sure axis is a33 Normalize vector
		double axisNorm = axis.Norm();
		if ( axisNorm == 0 )
			throw(new std::exception("Argument axis'S length can't be 0."));

		S = cos(angle/2);
		V = sin(angle/2) * axis / axisNorm;
	}

	Quaternion::Quaternion(const double s_in, const double v1, const double v2, const double v3)
	{
		S = s_in;
		V = Vector3(v1,v2,v3);
	}

	Quaternion::Quaternion(const Matrix & a33)
	{
		if((a33.GetColumn() != 3) && (a33.GetRow() != 3))
			throw(new std::exception("Argument a33's row and column count must be 3."));

		S  = 1.0;
		V  = ColumnVector(3);
		V.SetZero();

		double trace = a33(0,0) + a33(1,1) + a33(2,2) + 1.0;
		if( trace > Epsilon )
		{
			double s = 0.5 / sqrt(trace);
			S = 0.25 / s;
			V[0] = ( a33(2,1) - a33(1,2) ) * s;
			V[1] = ( a33(0,2) - a33(2,0) ) * s;
			V[2] = ( a33(1,0) - a33(0,1) ) * s;
		}
		else
		{
			if ( a33(0,0) > a33(1,1) && a33(0,0) > a33(2,2) ) 
			{
				double s = 2.0 * sqrt( 1.0 + a33(0,0) - a33(1,1) - a33(2,2));
				S = (a33(2,1) - a33(1,2) ) / s;
				V[0] = 0.25 * s;
				V[1]= (a33(0,1) + a33(1,0) ) / s;
				V[2] = (a33(0,2) + a33(2,0) ) / s;
			} 
			else if (a33(1,1) > a33(2,2))
			{
				double s = 2.0 * sqrt( 1.0 + a33(1,1) - a33(0,0) - a33(2,2));
				S = (a33(0,2) - a33(2,0) ) / s;
				V[0] = (a33(0,1) + a33(1,0) ) / s;
				V[1] = 0.25 * s;
				V[2] = (a33(1,2) + a33(2,1) ) / s;
			} 
			else 
			{
				double s = 2.0 * sqrt( 1.0 + a33(2,2) - a33(0,0) - a33(1,1) );
				S = (a33(1,0) - a33(0,1) ) / s;
				V[0] = (a33(0,2) + a33(2,0) ) / s;
				V[1] = (a33(1,2) + a33(2,1) ) / s;
				V[2] = 0.25 * s;
			}
		}
	}

	Quaternion operator + (const Quaternion &lhs, const Quaternion & rhs)
	{
		Quaternion q;
		q.S = lhs.S + rhs.S;
		q.V = lhs.V + rhs.V;

		return q;
	}

	Quaternion operator - (const Quaternion &lhs, const Quaternion & rhs)
	{
		Quaternion q;
		q.S = lhs.S - rhs.S;
		q.V = lhs.V - rhs.V;

		return q;
	}

	Quaternion operator * (const Quaternion &lhs, const Quaternion & rhs) 
	{
		Quaternion q;
		q.S = lhs.S * rhs.S - lhs.V * rhs.V;
		q.V = lhs.S * rhs.V + rhs.S * lhs.V + CrossProd(lhs.V, rhs.V);

		return q;
	}


	Quaternion operator * (const double c, const Quaternion & q)
	{
		Quaternion out;
		out.S = q.S * c;
		out.V = q.V * c;
		return out;
	}

	Quaternion operator*(const Quaternion & q, const double c)
	{
		return operator*(c, q);
	}

	Quaternion   operator / (const Quaternion &lhs, const Quaternion & rhs)
	{
		return lhs * rhs.Inverse();
	}

	//Quaternion operator/(const double c, const Quaternion & q)
	//{
	//	Quaternion out;
	//	out.set_s(q.Get_s() / c);
	//	out.set_v(q.v() / c);
	//	return out;
	//}

	Quaternion operator / (const Quaternion & q, const double c)
	{
			Quaternion out;
			out.S = q.S / c;
			out.V = q.V / c;
			return out;
	}

	Quaternion Quaternion::Conjugate() const
	{
		Quaternion q;
		q.S = S;
		q.V = -1*V;

		return q;
	}

	double Quaternion::Norm()const 
	{ 
		return( sqrt(S*S + V * V) );
	}

	void Quaternion::Normalize()
	{
		double tmp = Norm();
		if(tmp > Epsilon)
		{
			S = S/ tmp;
			V = V/ tmp;
		}
	}

	Quaternion Quaternion::Inverse() const 
	{ 
		return Conjugate() / (S*S + V * V);
	}

	Quaternion Quaternion::Exp() const
	{
		Quaternion q;
		double theta = sqrt(V * V),
			sin_theta = sin(theta);

		q.S = cos(theta);
		if ( fabs(sin_theta) > Epsilon)
			q.V = V*sin_theta/theta;
		else
			q.V  = V;

		return q;
	}

	Quaternion Quaternion::Power(const double t) const
	{
		Quaternion q = (Log()*t).Exp();

		return q;
	}

	Quaternion Quaternion::Log()const
	{
		Quaternion q;
		q.S = 0;
		double theta = acos(S),
			sin_theta = sin(theta);

		if ( fabs(sin_theta) > Epsilon)
			q.V = V/sin_theta*theta;
		else
			q.V = V;

		return q;
	}

	Quaternion Quaternion::Dot(const ColumnVector & w, const int sign)const
	{
		Quaternion q;
		q.S = -0.5 * V * w;
		q.V = 0.5*E(sign)*w;
	
		return q;
	}
	
	Matrix Quaternion::E(const int sign)const
	{
		Matrix E(3,3);
		Matrix I(3,3);
		I = Rotation3::Identity();
	
		Matrix VMatrix(3,3);
		VMatrix.SetZero();
		VMatrix(0,0) =  0;    VMatrix(0,1) = -V[3]; VMatrix(0,2) =  V[2];
		VMatrix(1,0) =  V[3]; VMatrix(1,1) =  0;    VMatrix(1,2) = -V[1];
		VMatrix(2,0) = -V[2]; VMatrix(2,1) =  V[1]; VMatrix(2,2) =  0;

		E = S*I + VMatrix;
		if(sign == 1)
			E = S*I + VMatrix;
		else
			E = S*I - VMatrix;

		return E;
	}

	double Quaternion::DotProduct(const Quaternion & q)const
	{
		return S * q.S + V[0] * q.V[0] + V[1] * q.V[1] + V[2] * q.V[2] ;
	}

	Rotation3 Quaternion::Rotation() const
	{
		//Rotation3 identity = Rotation3::Identity();
		//Rotation3 temp1 = Rotation3(  - V[1] * V[1] - V[2] * V[2],		V[0] * V[1],								V[0] * V[2], 
		//														V[0] * V[1],								- V[0] * V[0] - V[2] * V[2],		V[1] * V[2], 
		//														V[0] * V[2],								V[1] * V[2],								- V[0] * V[0] - V[1] * V[1]);
		//Rotation3 temp2 = Rotation3(  0,		     -V[2],		V[1], 
		//														V[2],		 0,		    -V[0], 
		//														-V[1],	  V[0],		0);

		////Rotation3 res = identity - 2 * temp1 + 2 * this->S * temp2;
		//Rotation3 res = Rotation3();
		//for(int row = 0; row < 3; row++)
		//{
		//	for(int col = 0; col < 3; col++)
		//	{
		//		res(row, col) = identity(row, col) - 2 * temp1(row, col) + 2 * this->S * temp2(row, col);
		//	}
		//}

		//return res;

		Rotation3 rot = Rotation3::Identity();
		double sqw = S*S;    
		double sqx = V[0]*V[0];    
		double sqy = V[1]*V[1];  
		double sqz = V[2]*V[2];
		// invs (inverse square length) is only required if quaternion is not already normalised    
		double invs = 1 / (sqx + sqy + sqz + sqw);

		rot(0,0) = ( sqx - sqy - sqz + sqw)*invs; // since sqw + sqx + sqy + sqz =1/invs*invs    
		rot(1,1) = (-sqx + sqy - sqz + sqw)*invs; 
		rot(2,2) = (-sqx - sqy + sqz + sqw)*invs;  
		double tmp1 = V[0]*V[1];   
		double tmp2 = V[2]*S;   
		rot(1,0) = 2.0 * (tmp1 + tmp2)*invs;  
		rot(0,1) = 2.0 * (tmp1 - tmp2)*invs;     
		tmp1 = V[0]*V[2];  
		tmp2 = V[1]*S; 
		rot(2,0) = 2.0 * (tmp1 - tmp2)*invs; 
		rot(0,2) = 2.0 * (tmp1 + tmp2)*invs;
		tmp1 = V[1]*V[2];  
		tmp2 = V[0]*S;
		rot(2,1) = 2.0 * (tmp1 + tmp2)*invs; 
		rot(1,2) = 2.0 * (tmp1 - tmp2)*invs; 
		return rot;
	}

	Frame3 Quaternion::Transformation()const
	{
		Rotation3 rot = this->Rotation();
		Vector3 trans = Vector3::Zero();
		return Frame3(rot, trans);
	}

	//ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot)
	//{
	//	Matrix A, B, M;
	//	UpperTriangularMatrix U;
	//	ColumnVector w(3);
	//	A = 0.5*q.E(BASE_FRAME);
	//	B = q_dot.v();
	//	if(A.Determinant())
	//	{
	//		QRZ(A,U);             //QR decomposition
	//		QRZ(A,B,M);
	//		w = U.i()*M;
	//	}
	//	else
	//		w = 0;
	//
	//	//w.Release();
	//	return w;
	//}

	short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
					 Quaternion & quat, const double dt)
	{
		if (dt < 0)
			throw(new std::exception("Integ_Trap(quat1, quat2, dt): dt < 0. dt is set to 0."));

		// Quaternion algebraic constraint
		//  Real Klambda = 0.5*(1 - quat.norm_sqr());

		dquat_present.S = dquat_present.S ;//+ Klambda*quat.Get_s());
		dquat_present.V = dquat_present.V ; //+ Klambda*quat.v());

		quat.S = quat.S + Integ_Trap_quat_s(dquat_present, dquat_past, dt);
		quat.V = quat.V + Integ_Trap_quat_v(dquat_present, dquat_past, dt);

		dquat_past.S = dquat_present.S;
		dquat_past.V = dquat_present.V;

		quat.Normalize();

		return 0;
	}

	double Integ_Trap_quat_s(const Quaternion & present, Quaternion & past, const double dt)
	{
		double integ = 0.5 * ( present.S + past.S ) * dt;
		past.S = present.S;
		return integ;
	}

	ColumnVector Integ_Trap_quat_v(const Quaternion & present, Quaternion & past, const double dt)
	{
		ColumnVector integ = 0.5 * ( present.V + past.V ) * dt;
		past.V = present.V;
		return integ;
	}

	Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const double t)
	{
		if( (t < 0) || (t > 1) )
			throw(new std::exception("Slerp(q0, q1, t): t < 0 or t > 1. t is set to 0."));

		if(q0.DotProduct(q1) >= 0)
			return q0 * ( ( q0.Inverse() * q1 ).Power(t) );
		else
			return  q0 * ( ( q0.Inverse() * -1 *q1 ).Power(t) );
	}

	Quaternion Slerp_prime(const Quaternion & q0, const Quaternion & q1, const double t)
	{
		if( (t < 0) || (t > 1) )
			throw(new std::exception("Slerp_prime(q0, q1, t): t < 0 or t > 1. t is set to 0."));

		if(q0.DotProduct(q1) >= 0)
			return Slerp(q0, q1, t)*(q0.Inverse()*q1).Log();
		else
			return Slerp(q0, q1, t)*(q0.Inverse()*-1*q1).Log();
	}

	Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b, const Quaternion & q, const double t)
	{
		if( (t < 0) || (t > 1) )
			throw(new std::exception("Squad(p,a,b,q, t): t < 0 or t > 1. t is set to 0."));

		return Slerp( Slerp( p, q, t), Slerp( a, b, t), 2 * t * ( 1 - t ) );
	}

	Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b, const Quaternion & q, const double t)
	{
		if( (t < 0) || (t > 1) )
			throw(new std::exception("Squad_prime(p,a,b,q, t): t < 0 or t > 1. t is set to 0."));

		Quaternion q_squad,
			U = Slerp(p, q, t),
			V = Slerp(a, b, t),
			W = U.Inverse()*V,
			U_prime = U*(p.Inverse()*q).Log(),
			V_prime = V*(a.Inverse()*b).Log(),
			W_prime = U.Inverse()*V_prime - U.Power(-2)*U_prime*V;

		q_squad = U*( W.Power(2*t*(1-t))*W.Log()*(2-4*t) + W.Power(2*t*(1-t)-1)*W_prime*2*t*(1-t) )
			+ U_prime*( W.Power(2*t*(1-t)) );

		return q_squad;
	}

	bool Equal(const Quaternion& lhs,const Quaternion& rhs,const double eps)
	{
		if(Equal(lhs.S,rhs.S,eps) == false)
			return false;
		if(Equal(lhs.V,rhs.V,eps) == false)
			return false;

		return true;

	}
}}






